#include "algorytmyNumeryczne.h"
using namespace std;

void algNum::pochodne(double t, vector<double> y, vector<double>& dydx, Model model, int zaburzenie) {
    double pPoprz, p, pNast;
    int n = model.iloscBaz;
    
    model.przeliczSprzezenia(t, zaburzenie);
    /*
     * for(int i=0; i<n-1; ++i) {
     * pPoprz = (!i ? 0. : y[i-1]);
     * p = y[i];
     * pNast = (i+2==n ? 0. : y[i+1]);
     *
     * //        dydx[i] = -model.getW(i,i+1,t,zaburzenie)*(p-pNast)*(1-(i+2==n)) - model.getW(i,i-1,t,zaburzenie)*(p-pPoprz)*(1-(!i));// - model.getW(i,i+1,t,zaburzenie)*p*(i+2==n);
     * dydx[i] = -model.getW(i,i+1,t,zaburzenie)*(p-pNast);
     * //        cout<<"["<<(p-pNast)<<" * "<<model.getW(i,i+1,t,zaburzenie)<<"] ";
     * if(i) {
     * dydx[i] -= model.getW(i,i-1,t,zaburzenie)*(p-pPoprz);// - model.getW(i,i+1,t,zaburzenie)*p*(i+2==n);
     * //            cout<<" ["<<(p-pPoprz)<<" * "<<model.getW(i,i-1,t,zaburzenie)<<"] ";
     * }
     * //        cout<<"  ok: "<<i<<" :" <<dydx[i]<<" "<<y[i]<<endl;
     *
     * }
     * dydx[n-1]=model.getW(n-2,n-1,t,zaburzenie)*y[n-2];
     *
     * //    cout<<n<<endl;
     */
    
    dydx[0] = - y[0] * model.getW(0, 1, t, zaburzenie) + y[1] * model.getW(1, 0, t, zaburzenie);
    for(int i=1; i<n-1; ++i) {
        dydx[i] = -y[i]*(model.getW(i, i+1, t, zaburzenie) + model.getW(i, i-1, t, zaburzenie)) + y[i+1]*model.getW(i+1, i, t, zaburzenie) + y[i-1]*model.getW(i-1, i, t, zaburzenie);
    }
//    dydx[n-2] = -y[n-2]*(model.getW(n-2, n-1, t, zaburzenie) + model.getW(n-2, n-3, t, zaburzenie)) + y[n-3]*model.getW(n-3, n-2, t, zaburzenie);
//    dydx[n-1] = y[n-2] * model.getW(n-2, n-1, t, zaburzenie);
    dydx[n-1] = y[n-2] * model.getW(n-2, n-1, t, zaburzenie) - y[n-1] * model.getW(n-1, n-2, t, zaburzenie);
    
    for(int i=0; i<3; ++i) {
       dydx[n-i-1] -= y[n-i-1] * model.GAMMA;
    }
    
    
    /*
     * for(int i=0; i<n; ++i)
     * if(model.lancuch[i]=='G') {
     * for(int j=i+2; j<n; ++j) {
     * if(model.lancuch[j]=='G') {
     * //                    dydx[i] -= model.getW(i,j,t,zaburzenie)*(y[i]-y[j]);
     * dydx[i] += -y[i] * model.getW(i,j,t,zaburzenie);
     * if(j!=n-1) {
     * dydx[i] += y[j] * model.getW(j,i,t,zaburzenie);
     * dydx[j] += -y[j] * model.getW(j,i,t,zaburzenie) + y[i] * model.getW(i,j,t,zaburzenie);
     * }
     * //                    dydx[i] += -y[i] * model.getW(i,j,t,zaburzenie) + y[j] * model.getW(j,i,t,zaburzenie);
     * }
     * }
     * }
     */
    /*
     * if(dydx[0]>0. || dydx[1]<0.) {
     * cout<<"uwaga! pochodne: [0] "<<dydx[0]<<" [1] "<<dydx[1]<<endl;
     * cout<<"y: [0] "<<y[0]<<" [1] "<<y[1]<<" ; "<<endl;
     * cout<<"k:\n";
     * for(int i=0; i<n-1; ++i)
     * cout<<"["<<i<<"-"<<i+1<<"]"<<model.getW(i,i+1,t,zaburzenie)<<" ; ";
     * cout<<"\nk:\n";
     * for(int i=1; i<n; ++i)
     * cout<<"["<<i<<"-"<<i-1<<"]"<<model.getW(i,i-1,t,zaburzenie)<<" ; ";
     * cout<<endl;
     * exit(1);
     * }*/
}

void algNum::rk4(vector<double>& y, vector<double>& dydx, int n, double& x, double h, vector<double>& yout, Model model, int zaburzenie) {
    
    double xh, hh, h6, najwRoznica;
    vector<double> dym(n);
    vector<double> dyt(n);
    vector<double> yt(n, 0.);
    vector<double> calc(n);
    bool flag = false;
    
    while(1) {
        hh=h/2.;
        h6=h/6.;
        xh=x+hh;
        
        for (int i=0;i<n;++i) {
            yt[i]=y[i]+hh*dydx[i];
//            yt[i]=(yt[i]<0. ? -yt[i] : yt[i]);
        }
        algNum::pochodne(xh, yt, dyt, model, zaburzenie);
        
        for (int i=0;i<n;++i) {
            yt[i]=y[i]+hh*dyt[i];
//            yt[i]=(yt[i]<0. ? -yt[i] : yt[i]);
        }
        algNum::pochodne(xh, yt, dym, model, zaburzenie);
        
        for (int i=0;i<n;++i) {
            yt[i]=y[i]+h*dym[i];
//            yt[i]=(yt[i]<0. ? -yt[i] : yt[i]);
            dym[i]+=dyt[i];
        }
        algNum::pochodne(x+h, yt, dyt, model, zaburzenie);
        
        najwRoznica = 0.;
        flag = false;
        
        for (int i=0;i<n;++i) {
            calc[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);
            if(calc[i]<0.)
                flag = true;
//          yout[i]=y[i]+h6*(dydx[i]+dyt[i]+2.0*dym[i]);
            if(calc[i]-y[i]>najwRoznica)
                najwRoznica=calc[i]-y[i];
        }
        
//        cout << najwRoznica << " " << h << endl;
        
        if(najwRoznica > 0.01) {
            h = h * 0.009 / najwRoznica;
        }
        else if(flag) {
            h = h/2.;
        }
        else {
            /*
             * if((calc[0]-y[0])/h6>1.e-2)
             * cout<<"uwaga! "<<(calc[0]-y[0])/h6<<" "<<y[0]<<" "<<y[1]<<" "<<x<<endl;
             */
            break;
        }
    }
    
    for (int i=0;i<n;++i) {
        yout[i]=calc[i];
    }
    
    yt.clear();
    dyt.clear();
    dym.clear();
}

int algNum::iteruj(int nkrokow, Model& model, int zaburzenie, double& tTot) {
    double dt = DT;
    vector<double> y(model.iloscBaz);
    vector<double> dydt(model.iloscBaz);
    vector<double> yout(model.iloscBaz);
    
    for(int i=0;i<model.iloscBaz;++i)
        y[i]=model.getP(i);
    
    algNum::pochodne(tTot, y, dydt, model, zaburzenie);
    
    int ti;
//    for(ti=0; ti<nkrokow && y[model.iloscBaz-1]<limitAkceptor; ++ti) {
    for(ti=0; ti<nkrokow; ++ti) {
        
        algNum::rk4(y, dydt, model.iloscBaz, tTot, dt, yout, model, zaburzenie);
        tTot+=dt;
        for (int j=0; j<model.iloscBaz; ++j)
            y[j]=yout[j];
        algNum::pochodne(tTot, y, dydt, model, zaburzenie);
        
    }
    if(ti<nkrokow)
        return ti;
    for(int j=0; j<model.iloscBaz; ++j)
        model.P[j] = y[j];
    
    yout.clear();
    dydt.clear();
    y.clear();
    
    return -1;
}
